#include "api.h"
#include <iostream>
#include <unistd.h>
#include <sys/time.h>
#include <thread>
#include <unistd.h>
#ifdef PCL
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#endif
#include <assert.h>

using namespace std;

//#define IP "127.0.0.1"
#define IP "192.168.10.100"
// #define IP2 "192.168.10.101"

#define DEV "DEV:"<<DevID << " "


void save( char *fileName , char *data , int len)
{
    FILE *pFile;
    pFile = fopen( fileName , "w+");
    if(pFile == NULL)
    {
        return ;
    }
    fwrite(data, 1, len, pFile);
    fclose(pFile);
}


void test(void *ip )
{
    int DevID = 0;
    struct timeval startTime , endTime;
    STRC_IMG_ALL *pImg;
    int mNumFrame = 0;
    char *pInfo = NULL;
    char *mIP = (char *)ip;
    char mFileName[1024];
    
    cout << "SDK VER:" << api_get_sdk_ver() << endl;

    //connect
    DevID = api_connect((char *)mIP );
    cout << "DevID:" << DevID<< endl;
    if(DevID < 0)
    {
        cout << "api_connect failed." << endl;
        return ;
    }
    sleep(1);
    pInfo = api_get_dev_ver(DevID);
    if(pInfo == NULL)
    {
        cout << DEV << "get Sdk version failed." << endl;
    }else{
        cout << DEV << "SDK VER:" << pInfo << endl; 

    }

    pInfo = api_get_intrinsic_parameters(DevID);
    if(pInfo == NULL)
    {
        cout << DEV << "get device intrinsic parameters info failed." << endl;
    }else{
        cout << DEV << "Device intrinsic parameters:" << pInfo << endl; 

    }

    {//曝光参数
        int mCurExp , mMinExp , mMaxExp;
        mCurExp = api_get_exposure(DevID , &mMinExp , &mMaxExp);
        if(mCurExp < 0)
        {
            cout << DEV << "Get exposure parameter failed. error code:" << mCurExp << endl;
        }else{
            cout << DEV << "CurExp:"<<mCurExp << "  MinExp:" << mMinExp << "  MaxExp:"<<mMaxExp << endl;
        }
    }

    //tof SN
    while ((pInfo = api_get_sn_tof(DevID)) == NULL)
        ;
    cout << DEV << "TOF SN:" << pInfo << endl;



    while((pImg = api_get_img(DevID) ) == NULL);
    sprintf(mFileName , "img-rgb-%d.jpg", DevID);
    save(mFileName , (char *)pImg->img_rgb.data , pImg->img_rgb.len);
    sprintf(mFileName , "img-depth-%d.raw", DevID);
    save(mFileName , (char *)pImg->img_depth.data , pImg->img_depth.len);
    sprintf(mFileName , "img-amplitude-%d.raw", DevID);
    save(mFileName , (char *)pImg->img_amplitude.data , pImg->img_amplitude.len);

    {
        //设置曝光
        api_set_exposure(DevID , 30);
        {//曝光参数
            int mCurExp , mMinExp , mMaxExp;
            mCurExp = api_get_exposure(DevID , &mMinExp , &mMaxExp);
            if(mCurExp < 0)
            {
                cout << DEV << "Get exposure parameter failed. error code:" << mCurExp << endl;
            }else{
                cout << DEV << "CurExp:"<<mCurExp << "  MinExp:" << mMinExp << "  MaxExp:"<<mMaxExp << endl;
            }
        }
    }
    gettimeofday(&startTime , NULL);
    while(mNumFrame < 60)
    {
        if((pImg = api_get_img(DevID)) != NULL)
        {
            mNumFrame++;
            //cout << "size of rgb:" << pImg->img_rgb.len << endl;
            // cout << "size of depth:" << pImg->img_depth.len << endl;
            // cout << "size of amplitude:" << pImg->img_amplitude.len << endl;
        }
    }
    gettimeofday(&endTime , NULL);
    cout << DEV << "mNumFrame:" << mNumFrame << endl;
    cout << DEV << "sec:" << endTime.tv_sec - startTime.tv_sec << endl;
    cout << DEV << "usec:" << endTime.tv_usec - startTime.tv_usec << endl;

    cout << DEV << "fps: " << (int)(60/(endTime.tv_sec - startTime.tv_sec)) << endl;

    int index = 0;

    while(true)
    {
        if((pImg = api_get_img(DevID) ) == NULL)
        {
            usleep(10000);

        }else{
#ifdef PCL
            //测试转点云
            sprintf(mFileName, "pcl-%d-%d.ply", DevID, index);
            pcl::io::savePLYFile(mFileName, *(pImg->pCloud));
#endif //PCL
            //测试pcd
            sprintf(mFileName, "pcd-%d-%d.pcd", DevID, index);
            char *mDatapcd;
            int mLenPcd = api_depth2pcd(DevID, pImg->img_depth.data, &mDatapcd);
            save(mFileName , mDatapcd, mLenPcd);
            // 测度amplitude gamma
            if (index == 0)
            {
                sprintf(mFileName, "amplitude-gamma-def.raw");
                save(mFileName , (char *)pImg->img_amplitude.data, pImg->img_amplitude.len);
                api_set_gamma_amplitude(DevID , 0.3 , true);
            }else if(index == 1)
            {
                sprintf(mFileName, "amplitude-gamma-0.3.raw");
                save(mFileName, (char *)pImg->img_amplitude.data, pImg->img_amplitude.len);
                api_set_gamma_amplitude(DevID, 1.3, true);
            }else if(index == 2){
                sprintf(mFileName, "amplitude-gamma-1.3.raw");
                save(mFileName, (char *)pImg->img_amplitude.data, pImg->img_amplitude.len);
                break;
            }

            index++;
            if(index%30==0)
            {
                cout << "receive img:" << index << endl;
            }
        }
    }
    //测试阻塞模式
    api_set_blocking_mode(DevID, true);
    mNumFrame = 0;
    gettimeofday(&startTime, NULL);
    while (mNumFrame < 150)
    {
        pImg = api_get_img(DevID);
        assert(pImg->img_amplitude.len != 0);
        mNumFrame++;
    }
    gettimeofday(&endTime, NULL);
    cout << DEV << "blocking fps: " << (int)(150 / (endTime.tv_sec - startTime.tv_sec)) << endl;

    // 测试多帧合一
    api_set_xin1(DevID , 10);
    mNumFrame = 0;
    gettimeofday(&startTime, NULL);
    while (mNumFrame < 150)
    {
        pImg = api_get_img(DevID);
        mNumFrame++;
    }
    gettimeofday(&endTime, NULL);
    cout << DEV << "10in1 fps: " << (int)(150 / (endTime.tv_sec - startTime.tv_sec)) << endl;

    api_set_xin1(DevID, 5);
    mNumFrame = 0;
    gettimeofday(&startTime, NULL);
    while (mNumFrame < 150)
    {
        pImg = api_get_img(DevID);
        mNumFrame++;
    }
    gettimeofday(&endTime, NULL);
    cout << DEV << "5in1 fps: " << (int)(150 / (endTime.tv_sec - startTime.tv_sec)) << endl;
}
int main()
{
    int ret = 0;
    api_init();
    thread th1(test, (void *)IP);
    // thread th2(test , (void *)IP2);
    th1.join();
    // th2.join();

    return ret;
}
